/*
 * ELM327.c
 *
 */

#include "ELM327.h"
#include "task.h"
#include "memory.h"
#include "tool.h"
#include "console.h"
#include "xstring.h"
#include <stdio.h>

QBuf gELM327TxQBuf;
QBuf gELM327RxQBuf;


void ELM327_Init(void)
{
	GPIO_InitTypeDef		GPIO_InitStructure;
    USART_InitTypeDef		USART_InitStructure;
    USART_ClockInitTypeDef	USART_ClockInitStructure;
    EXTI_InitTypeDef		EXTI_InitStructure;
    NVIC_InitTypeDef		NVIC_InitStructure;
        
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOD, ENABLE);
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_UART5, ENABLE);
        
    /* ELM327 TX */
    GPIO_InitStructure.GPIO_Mode    =   GPIO_Mode_AF_PP;
    GPIO_InitStructure.GPIO_Speed	=   GPIO_Speed_50MHz;
    GPIO_InitStructure.GPIO_Pin     =   GPIO_Pin_12;
    GPIO_Init(GPIOC, &GPIO_InitStructure);
    
    /* ELM327 RX */
    GPIO_InitStructure.GPIO_Mode    =   GPIO_Mode_IN_FLOATING;
    GPIO_InitStructure.GPIO_Pin     =   GPIO_Pin_2;
    GPIO_Init(GPIOD, &GPIO_InitStructure);
	
    /* Enable the USART5 Interrupt */
	NVIC_InitStructure.NVIC_IRQChannel              		= UART5_IRQn;
	NVIC_InitStructure.NVIC_IRQChannelSubPriority 			= IRQ_UART5;
	NVIC_InitStructure.NVIC_IRQChannelCmd           		= ENABLE;
	NVIC_Init(&NVIC_InitStructure);
	    
	NVIC_InitStructure.NVIC_IRQChannel              		= EXTI15_10_IRQn;
	NVIC_InitStructure.NVIC_IRQChannelSubPriority 			= IRQ_EXTI_15_10;
	NVIC_InitStructure.NVIC_IRQChannelCmd           		= ENABLE;
	NVIC_Init(&NVIC_InitStructure);
	        
	/* EXTI Setting */      
	EXTI_ClearITPendingBit(EXTI_Line12);
    GPIO_EXTILineConfig(GPIO_PortSourceGPIOB, GPIO_PinSource12);
    EXTI_InitStructure.EXTI_Mode	= EXTI_Mode_Interrupt;
    EXTI_InitStructure.EXTI_Line	= EXTI_Line12;
    EXTI_InitStructure.EXTI_Trigger	= EXTI_Trigger_Rising_Falling;
    EXTI_InitStructure.EXTI_LineCmd	= ENABLE;
    EXTI_Init(&EXTI_InitStructure);
    EXTI_ClearITPendingBit(EXTI_Line12);
        
    /* USART2 Setting */
    USART_InitStructure.USART_BaudRate		= 38400;
    USART_InitStructure.USART_WordLength	= USART_WordLength_8b;
    USART_InitStructure.USART_StopBits		= USART_StopBits_1;
    USART_InitStructure.USART_Parity		= USART_Parity_No;
    USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
    USART_InitStructure.USART_Mode			= USART_Mode_Rx | USART_Mode_Tx;
        
        
    USART_ClockInitStructure.USART_Clock		= USART_Clock_Disable;
    USART_ClockInitStructure.USART_CPOL			= USART_CPOL_Low;
    USART_ClockInitStructure.USART_CPHA			= USART_CPHA_2Edge;
    USART_ClockInitStructure.USART_LastBit		= USART_LastBit_Disable;
        
    USART_Init(UART5, &USART_InitStructure);
    USART_ClockInit(UART5, &USART_ClockInitStructure);
    USART_ITConfig(UART5, USART_IT_RXNE, ENABLE);
    USART_Cmd(UART5, ENABLE);

    QBuf_Init(&gELM327TxQBuf, pELM327TxData, ELM327_TX_SIZE);
    QBuf_Init(&gELM327RxQBuf, pELM327RxData, ELM327_RX_SIZE);
    *pFlag_elm327_tx_busy = 0;
}

void ELM327_CMD_Send(int idx)
{
    switch(idx)
    {
    case ELM327_ATZ:
        ELM327_Write("ATZ\r", 4);
        break;
    case ELM327_ATSP0:
        ELM327_Write("ATSP0\r", 6);
        break;
    case ELM327_RPM:
        ELM327_Write("010C\r", 5);
        break;
    case ELM327_SPEED:
        ELM327_Write("010D\r", 5);
        break;
    case ELM327_ATDPN:
        ELM327_Write("ATDPN\r", 6);
        break;
    case ELM327_ATRV:
        ELM327_Write("ATRV\r", 5);
        break;
    case ELM327_TEMPRO:
        ELM327_Write("0105\r", 5);
        break;
    case ELM327_ATDP:
        ELM327_Write("ATDP\r", 5);
    case ELM327_ATCV1450:
        ELM327_Write("ATCV1450\r", 9);
        break;
    default:
        break;
    }
}

void ELM327_Write(char *buf, unsigned short count)
{
	unsigned short remain;
	char *temp;
	
	temp = buf;

	do
	{
		remain = gELM327TxQBuf.len - gELM327TxQBuf.use;
		if (count < remain)
		{
			remain = count;
		}
		count -= remain;
		QBuf_Write(&gELM327TxQBuf, temp, remain);
		temp += remain;
		if (*pFlag_elm327_tx_busy == 0)
		{
			*pFlag_elm327_tx_busy = 1;
            USART_ITConfig(UART5, USART_IT_TXE, ENABLE);
		}
	} while (count != 0);
    
}

static char elm_temp[128] = {0};
short elm327_speed = 0xFFFF;
unsigned long elm327_dbug = 0;
unsigned long elm327_dbug2 = 0;
unsigned long elm327_dbug3 = 0;
unsigned long elm327_dbug4 = 0;
unsigned char elm327_timeout = 0;

void ELM327_Task(void)
{
    static int elm327_cmd_idx = 0;

	if(elm327_speed == -1)
	{
		if(elm327_timeout < 200)
		{
			elm327_timeout++;
		}
		else
		{
			elm327_timeout = 0;
			//ELM327_CMD_Send(ELM327_ATZ);
			elm327_cmd_idx = ELM327_ATZ;
			*pFlag_ELM327_wait_res = 0;
			*pFlag_ELM327_ready = 0;
		}
	}
    
    if(*pFlag_ELM327_wait_res == 0)
    {
        switch(elm327_cmd_idx)
        {
        case ELM327_ATZ:
            ELM327_CMD_Send(ELM327_ATZ);
            elm327_cmd_idx = ELM327_ATSP0;
            *pFlag_ELM327_wait_res = 1;
            break;
                
        case ELM327_ATSP0:
            ELM327_CMD_Send(ELM327_ATSP0);
            elm327_cmd_idx = ELM327_SPEED;
            *pFlag_ELM327_wait_res = 1;
            //*pFlag_ELM327_ready = 1;
            break;
			
		case ELM327_SPEED:
			ELM327_CMD_Send(ELM327_SPEED);
            elm327_cmd_idx = ELM327_WAIT;
            *pFlag_ELM327_wait_res = 1;
			break;
			
		case ELM327_WAIT:
			if(elm327_speed != -1)
				*pFlag_ELM327_ready = 1;
			elm327_cmd_idx = ELM327_OK;
			break;
			
        default:
            break;
        }
    }
    
    if (gELM327RxQBuf.use != 0)
    {
        int i;
        unsigned short len = gELM327RxQBuf.use;
		
        if( (len + gELM327RxQBuf.tail) < gELM327RxQBuf.len )
        {
            for(i = gELM327RxQBuf.tail; i < (gELM327RxQBuf.tail+len); i++)
            {
                if(gELM327RxQBuf.buf[i] == 0x3E)
                {
                    len = i+1-gELM327RxQBuf.tail;
                    QBuf_Read(&gELM327RxQBuf, elm_temp, len);
					*pFlag_ELM327_wait_res = 0;
                    break;
                }
            }
        }
        else
        {
            for(i = gELM327RxQBuf.tail; i < (gELM327RxQBuf.len); i++)
            {
                if(gELM327RxQBuf.buf[i] == 0x3E)
                {
                    len = i+1-gELM327RxQBuf.tail;
                    QBuf_Read(&gELM327RxQBuf, elm_temp, len);
					*pFlag_ELM327_wait_res = 0;
                    break;
                }
            }
            if(i == gELM327RxQBuf.len)
            {
                for(i = 0; i <= gELM327RxQBuf.head; i++)
                {
                    if(gELM327RxQBuf.buf[i] == 0x3E)
                    {
                        len = len-gELM327RxQBuf.head+i;
                        QBuf_Read(&gELM327RxQBuf, elm_temp, len);
						*pFlag_ELM327_wait_res = 0;
                        break;
                    }
                }
            }
        }
		
		/*
		//fenris 20110325 for elm327 Tx/Rx debug
		Console_Write(elm_temp, xstrlen(elm_temp));
		if(xstrlen(elm_temp) > 5)
			Console_Write("\r\n", 2);
		*/
		
		if(strncmp(elm_temp, " STOPPED", 7) == 0)
		{
			elm327_cmd_idx = ELM327_ATZ;
			*pFlag_ELM327_wait_res = 0;
			*pFlag_ELM327_ready = 0;
		}
        
        for(i = 4; i < len-1; i++)
        {
            if( (elm_temp[i-4] == '4') && (elm_temp[i-3] == '1') && (elm_temp[i-2] == ' ') && (elm_temp[i-1] == '0') && (elm_temp[i] == 'D') )
            {
                char str2int[3] = {0};
                str2int[0] = elm_temp[i+2];
                str2int[1] = elm_temp[i+3];
                elm327_speed = octstr_get_char(str2int);
                elm327_dbug++;
                elm327_dbug4 = elm327_dbug2 - elm327_dbug;
                break;
                       
            }
        }
            
        if(pCMD->ubxdtpt.mode == 200)
        {
            if(strlen(elm_temp) > 6)
            {
                Console_Write("\r\n", 2);
                Console_Write(elm_temp, xstrlen(elm_temp));
                sprintf(elm_temp, "\r\nSpeed: %d,%d,%d,%d\r\n", elm327_speed, elm327_dbug, elm327_dbug2, gSystick);
                Console_Write(elm_temp, xstrlen(elm_temp));
            }
        }
        
        if(pCMD->ubxdtpt.mode == 201)
        {
            Console_Write("\r\n", 2);
            Console_Write(elm_temp, xstrlen(elm_temp));
            sprintf(elm_temp, "\r\nSpeed: %d\r\n", elm327_speed);
            Console_Write(elm_temp, xstrlen(elm_temp));
        }
		
        //elm_temp[0] = 0;
        //*pFlag_ELM327_wait_res = 0;
        
        for(i = 0; i < 128; i++)
            elm_temp[i] = 0;
    }
	
}

void ELM327_UART_IRQ_Tx_Callback(void)
{
	char buf;

	if (gELM327TxQBuf.use != 0)
	{
		QBuf_Read(&gELM327TxQBuf, &buf, 1);
        USART_SendData(UART5, buf);
	}
	else
	{
        USART_ITConfig(UART5, USART_IT_TXE, DISABLE);
		*pFlag_elm327_tx_busy = 0;
	}
}

void ELM327_UART_IRQ_Rx_Callback(void)
{
	char buf;

    buf = USART_ReceiveData(UART5);
    QBuf_Write(&gELM327RxQBuf, &buf, 1);
}

void ELM327_Parser(char *buf, unsigned short count)
{
  
}
